#include <PID_v1.h>

// 定义传感器引脚
#define LEFT_SENSOR 16
#define CENTER_SENSOR 18
#define RIGHT_SENSOR 17

// 定义电机引脚
#define MOTOR_A1 5
#define MOTOR_A2 6
#define MOTOR_B1 9
#define MOTOR_B2 10

//定义黑色
#define black !(digitalRead(16))&&!(digitalRead(17))&&!(digitalRead(18))
//定义白色
#define white digitalRead(16)&&digitalRead(17)&&digitalRead(18)

// 设置PID参数
double Setpoint = 0; // 目标值为0，表示车辆应当保持在中心位置
double Kp = 2;       // 比例系数
double Ki = 5;       // 积分系数
double Kd = 1;       // 微分系数

double Input, Output;
PID pid(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
//定义传感器
int leftSensorValue, centerSensorValue, rightSensorValue;

//定义初始关卡为0
int level=0;
//定义初始速度
int begin_speed=200;
//定义左轮速度
int leftSpeed=200;
//定义右轮速度
int rightSpeed=200;

void setup() {
  Serial.begin(9600);
  // 配置电机引脚为输出模式
  pinMode(MOTOR_A1, OUTPUT);
  pinMode(MOTOR_A2, OUTPUT);
  pinMode(MOTOR_B1, OUTPUT);
  pinMode(MOTOR_B2, OUTPUT);
  // 配置传感器引脚为输入模式
  pinMode(CENTER_SENSOR, INPUT);
  pinMode(LEFT_SENSOR, INPUT);
  pinMode(RIGHT_SENSOR, INPUT);

  // 初始化PID控制器
  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-255, 255);

  // 输出PID参数信息
  Serial.println("PID Tuning:");
  Serial.print("Kp: ");
  Serial.println(Kp);
  Serial.print("Ki: ");
  Serial.println(Ki);
  Serial.print("Kd: ");
  Serial.println(Kd);

  //定义初始关卡为0
  int level=0;
}

void loop()
{
  while(1)
  {
    levelnum();//判断处于哪个关卡
    switch(level){
      case 0:
        run();
        break;
      case 1:
        run();
        break;
      case 2:
        run();
        break;
      case 3:
        run();
        break;
      case 4:
        run();
        break;
      case 5:
        run();
        break;
      case 6:
        run();
        break;
      case 7:
        run();
        break;
      case 8:
        run();
        break;
     }
    }
}

//关卡判断
void levelnum(){
  if(black&&level==0){  //判断第一关 全黑且关卡为0
    level=1;    
  }else if(white&&level==1){  //判断第二关 全白且关卡为1
    level=2;
  }else if(white&&level==2){ //判断第三关 全白且关卡为2
    level=3;  
  } else if(black&&level==3){ //判断第四关 全黑且关卡为3
    level=4; 
  }else if(white&&level==4){  //判断第五关 全白且关卡为4
    level=5;
  }else if(white&&level==5){ //判断第六关 全白且关卡为5
    level=6;  
  } else if(black&&level==6){ //判断第七关 全黑且关卡为6
    level=7; 
  }else if(black&&level==7){  //判断第八关 全黑且关卡为7
    level=8; 
  }
}

//计算pid
void pidcal(){
  // 读取传感器数据
  leftSensorValue = digitalRead(LEFT_SENSOR);
  centerSensorValue = digitalRead(CENTER_SENSOR);
  rightSensorValue = digitalRead(RIGHT_SENSOR);
  // 计算传感器数据的误差
  double error = 0.5 * (double)(rightSensorValue - leftSensorValue) + (double)centerSensorValue - 0.5;
  // 将误差归一化到 [-1, 1] 范围内
  Input = error / 1023.0 * 2.0;
  // 使用PID控制器计算输出
  pid.Compute();
  // 根据PID输出来控制电机转向
  leftSpeed = constrain(begin_speed + Output, 0, 255); // 左轮速度
  rightSpeed = constrain(begin_speed - Output, 0, 255); // 右轮速度
}

//前进
void run() {
   pidcal(); //计算pid
//   if(black){
//    str();//全黑 直行
//      }else
    if(!(digitalRead(16))){ 
    tleft();// 控制右轮转速增加，左转
      }else if(!(digitalRead(17))){  
    tright();// 控制左轮转速增加，右转
//      }else 
//      if(white){
//    stop();//全白 停止
      }else{
    str();//直行
      }
  // 输出调试信息
  Serial.print("Input: ");
  Serial.print(Input);
  Serial.print(" Output: ");
  Serial.println(Output);
  delay(10); // 延迟一段时间
}
//停止函数
void stop()
{
  digitalWrite(MOTOR_A1, LOW);
  digitalWrite(MOTOR_A2, LOW);
  digitalWrite(MOTOR_B1, LOW);
  digitalWrite(MOTOR_B2, LOW);
}
//直行函数
void str()
{
  analogWrite(MOTOR_A1, leftSpeed);
  digitalWrite(MOTOR_A2, LOW);
  analogWrite(MOTOR_B1, rightSpeed);
  digitalWrite(MOTOR_B2, LOW);    
}
//右转函数
void tright(){
  digitalWrite(MOTOR_A1, LOW);
  analogWrite(MOTOR_A2, leftSpeed-50);
  analogWrite(MOTOR_B1, rightSpeed-50);
  digitalWrite(MOTOR_B2, LOW);
  }
//左转函数
void tleft(){
  analogWrite(MOTOR_A1, leftSpeed-50);
  digitalWrite(MOTOR_A2, LOW);
  digitalWrite(MOTOR_B1, LOW);
  analogWrite(MOTOR_B2, rightSpeed-50);
  }
//调节转速函数，xy两个引脚，speed控制转速
void spin(int x,int y,int speed)
{
  analogWrite( x , speed );
  analogWrite( y , LOW );
}